import sensor, image, time, pyb, lcd, math
from pyb import UART
from pyb import LED
usart3=UART(3,115200)
usart3.init(115200, bits=8, parity=None, stop=1)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.set_framesize(sensor.QVGA)
sensor.set_windowing((20,0,242,240))
sensor.skip_frames(time = 2000)
lcd.init()
clock = time.clock()
LED(3).on()

fd="/cc.txt"
fc="/dd.txt"

DiTuList=[]
DiTuLists=[]

DiTuLists333=[]
DiTuList333=[]

f=open(fc,"r")
DiTus=f.readlines()
for i in DiTus:
    curline=i.strip().split(" ")
    DiTuLists.append(curline[:])

if len(DiTuLists)==11:
    for ii in range(10):
        DiTuList.append(int(DiTuLists[ii+1][0]))  #读取并转换已保存的地图数据
f.close()

f=open(fd,"r")
DiTus=f.readlines()
for i in DiTus:
    curline=i.strip().split(" ")
    DiTuLists333.append(curline[:])

if len(DiTuLists333)==11:
    for ii in range(10):
        DiTuList333.append(int(DiTuLists333[ii+1][0]))  #读取并转换已保存的地图数据
f.close()

DiTuCnt=0  #绘图进度变量
a=0        #串口数据
Mode=0     #模式选择
red =(26, 61, -9, 47, 2, 52)
red2=(45, 73, 18, 64, 0, 38)
xybuff=[]  #绘图点坐标储存
k=0        #显示进度变量
BaoGuangFlag=0
task=0


def UART_Out1(Dian):
    data = bytearray([0xb3, 0xb3, int(Dian[0]), int(Dian[1]), int(Dian[2]),
                                  int(Dian[3]), int(Dian[4]), int(Dian[5]),
                                  int(Dian[6]), int(Dian[7]), int(Dian[8]),0x5b])
    usart3.write(data)

def UART_Out2(redxy):
    data = bytearray([0xb3, 0xb3, int(redxy[0]), int(redxy[1]), 0x5b])
    usart3.write(data)

def UART_Out3(Dian, fl):
    data = bytearray([0xb3, 0xb3, int(Dian[0]), int(Dian[1]), int(Dian[2]),
                                  int(Dian[3]), int(Dian[4]), int(Dian[5]),
                                  int(Dian[6]), int(Dian[7]), int(Dian[8]),
                                  int(Dian[9]), int(fl), 0x5b])
    usart3.write(data)

while(True):
    clock.tick()
    img = sensor.snapshot().lens_corr(strength = 1.4)

    if Mode==2:    #绘图模式
        sensor.set_auto_exposure(False, exposure_us=5000)
        sensor.skip_frames(time = 1000)
        LED(1).on()
        LED(2).on()
        LED(3).on()
        while(True):
            k=0
            img = sensor.snapshot().lens_corr(strength = 1.4)
            if DiTuCnt>4:  #绘图完成
                while(True):
                    k=0
                    img = sensor.snapshot()
                    img.draw_string(0,0,"OK",color = (255, 255, 0), scale = 4)
                    for i in range(5):
                        img.draw_circle(xybuff[k], xybuff[k+1], 5, color =(0,0,255), fill=True)
                        k+=2
                    lcd.display(img)

            r_blobs = img.find_blobs([red],x_stride =1,y_stride =1,area_threshold =1,pixels_threshold =1)  #找红色激光点
            if r_blobs:
                img.draw_rectangle(r_blobs[0].rect(),color =(255,0,0))
                redxy=[r_blobs[0].cx(), r_blobs[0].cy()]

                if usart3.any()>0:  #绘制坐标点
                    a=usart3.readchar()
                    if a==0x04:
                        xybuff.append(r_blobs[0].cx())
                        xybuff.append(r_blobs[0].cy())
                        DiTuCnt+=1

            if DiTuCnt>4:
                if task==0:
                    f=open(fc,"w+")
                elif task==1:
                    f=open(fd,"w+")

                for kk in range(10):
                    f.write('\n'+str(xybuff[kk]))
                f.close()

            if DiTuCnt>0:  #显示绘图进度
                for i in range(DiTuCnt):
                    img.draw_circle(xybuff[k], xybuff[k+1], 5, color =(0,0,255), fill=True)
                    k+=2
            lcd.display(img)

    if Mode==0:   #找图模式
        k=0
        if len(DiTuLists)==11:
            for i in range(5):
                img.draw_circle(DiTuList[k], DiTuList[k+1], 5, color =(0,0,255), fill=True)
                k+=2
            UART_Out3(DiTuList, 0x02)
        else:
            img.draw_string(0,20,"ERR2",color = (255, 255, 0), scale = 2)

        k=0
        if len(DiTuLists333)==11:
            for i in range(5):
                img.draw_circle(DiTuList333[k], DiTuList333[k+1], 5, color =(0,0,255), fill=True)
                k+=2
            UART_Out3(DiTuList333, 0x03)
        else:
            img.draw_string(0,40,"ERR3",color = (255, 255, 0), scale = 2)

        if usart3.any()>0:
            a=usart3.readchar()
            if a==0x02:
                Mode=1
            if a==0x04:
                Mode=2
                task=0
            if a==0x05:
                Mode=2
                task=1
            if a==0x01:
                Mode=3

    if Mode==1:    #找矩形模式
        duanbian=0
        Dian=[]
        juxing=img.find_rects(threshold = 90000)
        if juxing:
            kuang=juxing[0].corners()
            changdu1=math.sqrt( ((kuang[0][0]-kuang[1][0])*(kuang[0][0]-kuang[1][0]))
                                + (kuang[0][1]-kuang[1][1])*(kuang[0][1]-kuang[1][1]) )
            changdu2=math.sqrt( ((kuang[0][0]-kuang[3][0])*(kuang[0][0]-kuang[3][0]))
                                + (kuang[0][1]-kuang[3][1])*(kuang[0][1]-kuang[3][1]) )
            if changdu1>changdu2:
                duanbian=changdu2
            else:
                duanbian=changdu1
            if duanbian>68:
                Dian.append(0x01)
            else:
                Dian.append(0x02)
            img.draw_rectangle(juxing[0].rect(), color = (255, 0, 0))
            for p in juxing[0].corners():
                img.draw_circle(p[0], p[1], 5, color = (0, 255, 0))
                Dian.append(p[0])
                Dian.append(p[1])
            UART_Out1(Dian)
            print(Dian)
        if usart3.any()>0:
            a=usart3.readchar()
            if a==0x01:
                Mode=3


    if Mode==3:  #找红激光模式
        if BaoGuangFlag==0:
            sensor.set_auto_exposure(False, exposure_us=5000)
            sensor.skip_frames(time = 1000)
            BaoGuangFlag=1

        r_blobs = img.find_blobs([red],x_stride =1,y_stride =1,area_threshold =1,pixels_threshold =1)
        if r_blobs:
            img.draw_rectangle(r_blobs[0].rect(),color =(255,0,0))
            redxy=[r_blobs[0].cx(), r_blobs[0].cy()]
            print(redxy)
            UART_Out2(redxy)
        else:
            red2_blobs = img.find_blobs([red2],x_stride =3,y_stride =3,area_threshold =10,pixels_threshold =10)
            if red2_blobs:
                if red2_blobs[0].pixels()<100:
                    img.draw_rectangle(red2_blobs[0].rect(),color =(0,255,0))
                    redxy=[red2_blobs[0].cx(), red2_blobs[0].cy()]
                    print(red2_blobs[0])
                    UART_Out2(redxy)
        if usart3.any()>0:
            a=usart3.readchar()
            if a==0x02:
                Mode=1
            if a==0x04:
                Mode=2
                task=0
            if a==0x05:
                Mode=2
                task=1



    img.draw_string(0,0,"FPS:%d"%(int(clock.fps())),color = (255, 255, 0), scale = 2)
    lcd.display(img)
